import os, sys
import cv2
import time
import numpy as np
import threading
import platform
from serial import Serial

import logging

logger = logging.getLogger("ssjhs")


class Camera_CDC_ACM:
    def __init__(self):
        from Driver.SerialPort import DeviceMap, SerialPort

        self.device_name = "Camera"

        self.__lock_sp = threading.Lock()

        if platform.system().lower() == "linux":
            self.port = DeviceMap["Camera"]
        else:
            self.port = "COM13"

        logger.info(f"{format(self.__class__.__name__)} Port: {self.port}")

        self.__sp = SerialPort(self.port)
        self.__sp.setName(self.device_name)
        self.__sp.set_timeout(None)

    def read_dump_bytes(self):
        self.__sp.set_timeout(0.1)
        dump_bytes = self.__sp.readData(0x10000)
        # logger.debug(f"dump_bytes length: {len(dump_bytes)}")
        self.__sp.set_timeout(None)

    def setTimeout(self, t):
        self.__sp.set_timeout(t)

    def write(self, cmd):
        try:
            self.__lock_sp.acquire()
            self.__sp.write(cmd)
            self.__lock_sp.release()
        except Exception as e:
            self.__lock_sp.release()
            raise e

    def read(self, cmd):
        try:
            self.__lock_sp.acquire()
            result = self.__sp.read(cmd)
            self.__lock_sp.release()
        except Exception as e:
            self.__lock_sp.release()
            raise e

        return result

    def check_version(self):
        version = None
        try:
            self.__sp.set_timeout(2)
            v = self.read("version")
            bv = v >> 16
            sv = v & 0xFFFF
            version = f"{bv}.{sv}"
            self.__sp.set_timeout(None)
            logger.info(f"Camera version: {version}")
        except Exception as e:
            raise Exception("Camera usb error")

    def start_streaming(self):
        self.__sp.write("start_streaming")

    def stop_streaming(self):
        self.__sp.write("stop_streaming")

    def get_frame(self):
        import cv2

        cmd = f"get_frame"
        self.__sp.writeOnly(cmd)
        text = self.__sp.readUntil(f"{cmd}\r\n")

        bytes_data = self.__sp.readData(4)
        length = np.frombuffer(bytes_data, dtype="<u4")[0]  # '<u4'表示小端序uint32
        logger.debug(f"jpeg length:{length}")
        if length == 0:
            return None, 4

        bytes_data = self.__sp.readData(length)
        dump_data = self.__sp.readOnly()

        bytes_data = bytes_data.rstrip(b'\x00')
        end_bytes = bytes_data[-2:]
        if end_bytes != b'\xff\xd9':
            logger.warning("Corrupt JPEG data: 29 extraneous bytes before marker 0xd9")
            print(bytes_data[-10:])
            return None, 4
    
        image_array = np.frombuffer(bytes_data, dtype=np.uint8)

        # n = 640 * 10
        # bytes_data = self.__sp.readData(n)
        # for i in range(n, length[0], n):
        #     bytes_data += self.__sp.readData(n)
        # self.__sp.readOnly()
        # image_array = np.frombuffer(bytes_data, dtype=np.uint8)

        # img = np.reshape(image_array, (480, 640))

        # 解码为OpenCV图像
        img = cv2.imdecode(image_array, -1)

        raw_data_len = length + 4
        return img, raw_data_len


############################################################################
if __name__ in os.path.basename(os.path.dirname(__file__)) + "." + os.path.splitext(os.path.basename(__file__))[0]:
    camera_cdc_acm = Camera_CDC_ACM()


############################################################################
if __name__ == "__main__":
    from Utilities import log

    log.setupLog()

    from camera_cdc_acm import camera_cdc_acm

    camera_cdc_acm.read_dump_bytes()
    camera_cdc_acm.check_version()
    camera_cdc_acm.start_streaming()

    ts = time.time()
    fps_count = 0
    fps = 0
    data_count = 0
    rate_of_flow = 0
    while True:
        frame, raw_data_len = camera_cdc_acm.get_frame()
        if frame is None:
            continue
        img_bgr = cv2.cvtColor(frame, cv2.COLOR_GRAY2BGR)

        fps_count += 1
        data_count += raw_data_len
        if fps_count >= 30:
            t = time.time() - ts
            fps = round(fps_count / t, 0)
            rate_of_flow = round(data_count / t)

            # print("fps:%d" % (fps))
            fps_count = 0
            data_count = 0
            ts = time.time()
        cv2.putText(img_bgr, f"FPS: {fps}", (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (20, 250, 20), 2)
        cv2.putText(img_bgr, f"rate_of_flow: {rate_of_flow/1000:.1f} KB/S", (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (200, 20, 200), 2)

        cv2.imshow("Decoded Image", img_bgr)
        cv2.waitKey(1)

    import os, sys

    device = "/dev/ttyACM0"
    sp = Serial(device)
    # sp.set_timeout(4)
    i = 0
    sp.write("E".encode())
    sp.timeout = 0.2
    dump_bytes = sp.read(100000)
    sp.timeout = None

    ts = time.time()
    fps_count = 0
    fps = 0
    while True:
        try:
            sp.write("B".encode())
            bytes_data = sp.read(4)
            length = np.frombuffer(bytes_data, dtype="<u4")  # '<u4'表示小端序uint32
            logger.debug(f"jpeg length:{length}")
            bytes_data = sp.read(length[0])
            data_array = np.frombuffer(bytes_data, dtype=np.uint8)
            img = cv2.imdecode(data_array, -1)

            img_bgr = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
            fps_count += 1
            if fps_count >= 30:
                t = time.time() - ts
                fps = int(round(fps_count / t, 0))
                # print("fps:%d" % (fps))
                fps_count = 0
                ts = time.time()
            cv2.putText(img_bgr, "FPS:" + str(fps), (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (200, 250, 200), 2)

            cv2.imshow("Decoded Image", img_bgr)
            cv2.waitKey(1)

        except Exception as e:
            print(e)

    sp.close()
